#include "../include/7Dof_Model_dyna.h"

#include "ros/ros.h"
#include "../include/serial_struct.h"
#include "../include/hardware/robot.h"

#include <math.h>
#include <iostream>
#include <thread>
#include <condition_variable>

#include <sensor_msgs/Joy.h>
#include <sensor_msgs/JointState.h>

#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>

using std::cerr;
using std::cout;
using std::endl;
using std::exception;
using std::string;
using std::vector;
using namespace std;
using namespace Eigen;

/* action 服务端声明 */
typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> ArmServer;

// 过渡
void now2start(float nowq[2], float startq[2], float q1Trace[1000], float q2Trace[1000])
{
      float dltq[2] = {0}, stepq[2] = {0};
      for (size_t i = 0; i < 2; i++)
      {
            dltq[i] = startq[i] - nowq[i];
            stepq[i] = dltq[i] / 1000.0;
      }

      for (size_t i = 0; i < 1000; i++)
      {
            q1Trace[i] = nowq[0] + i * stepq[0];
            q2Trace[i] = nowq[1] + i * stepq[1];
      }
}

// 全局变量
std::vector<FixedStepInterpolator> trajectory_segments;
int current_segment = 0;
int current_step = 0;
bool has_trajectory = false;

// 轨迹回调函数
void executeTrajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal, ArmServer *as)
{
      trajectory_segments.clear();

      // 每段轨迹按5ms步长插值
      for (size_t i = 0; i < goal->trajectory.points.size() - 1; ++i)
      {
            FixedStepInterpolator segment;
            double duration_ms = (goal->trajectory.points[i + 1].time_from_start -
                                  goal->trajectory.points[i].time_from_start)
                                     .toSec() *
                                 1000;
            int steps = std::max(1, (int)(duration_ms / 5)); // 至少1步

            segment.init(
                goal->trajectory.points[i].positions,
                goal->trajectory.points[i + 1].positions,
                steps);
            trajectory_segments.push_back(segment);
      }

      current_segment = 0;
      current_step = 0;
      has_trajectory = true;
      as->setSucceeded();
      ROS_INFO("New trajectory with %zu segments", trajectory_segments.size());
}

int main(int argc, char **argv)
{
      ros::init(argc, argv, "arm_7dof_driver");
      ros::NodeHandle n;
      // 控制循环
      ros::Rate r(200);

      lively_robot::robot rb; // 建一个机器人，包括板子，端口以及串口的线程打开等操作
      ROS_INFO("\033[1;32mSTART\033[0m");

      // --------------------------Topic----------------------------------
      sensor_msgs::JointState joint_state; // 需要发布的角度数据
      // ros::Subscriber sub = n.subscribe("/arm_7dof_driver/JointPos", 10, jointPosCallback);
      ros::Publisher pub = n.advertise<sensor_msgs::JointState>("/arm_7dof/joint_states", 1);
      // --------------------------Topic----------------------------------

      // --------------------------Action----------------------------------
      ArmServer as_(n, "arm_7dof/arm_controller/follow_joint_trajectory", boost::bind(&executeTrajectory, _1, &as_), false);
      // 启动 Action Server
      as_.start();
      ROS_INFO("Trajectory Action Server started");
      // --------------------------Action----------------------------------

      // 各关节方向系数（1.0或-1.0）
      const float joint_direction[7] = {-1.0, -1.0, -1.0, 1.0, -1.0, -1.0, -1.0}; // 请根据实际情况修改
      // 电机的位置
      float joint_pos[7] = {0, 0, 0, 0, 0, 0, 0};
      // 获取电机数据
      motor_back_t joint_motor[7];

      // 安全检查
      // 获取电机当前状态
      // 初始化电机（7轴）
      uint8_t safe_check_flag = 1;
      if (safe_check_flag)
      {
            // 电机返回值（7个关节）
            double joint[7] = {0};
            for (size_t i = 0; i < 30; i++)
            {
                  for (int j = 0; j < 7; j++)
                  {
                        rb.Motors[j]->fresh_cmd(0.0, 0.0, 0.0, 0.0, 0.0);
                  }
                  rb.motor_send();
                  r.sleep();
            }
            // 获取所有关节状态
            for (int j = 0; j < 7; j++)
            {
                  joint_motor[j] = *rb.Motors[j]->get_current_motor_state();
                  joint[j] = joint_direction[j] * joint_motor[j].position;
            }

            // 检查所有关节的绝对值是否都小于0.2
            bool all_joints_safe = true;
            for (int j = 0; j < 7; j++)
            {
                  if (fabs(joint[j]) >= 0.2)
                  {
                        all_joints_safe = false;
                        break;
                  }
            }

            // 如果有关节不满足条件，跳出
            if (!all_joints_safe)
            {
                  ROS_ERROR("Motor position init error!");
                  // // 打印所有关节位置
                  ROS_INFO("Positions: %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
                           joint[0], joint[1], joint[2], joint[3], joint[4], joint[5], joint[6]);
                  while (ros::ok())
                        ;
            }
      }

      // 获取电机当前状态
      while (ros::ok())
      {
            if (has_trajectory)
            {
                  // 获取当前插值点
                  auto positions = trajectory_segments[current_segment].getPosition(current_step);

                  // 电机执行
                  ROS_INFO_THROTTLE(0.1, "Pos: %.2f %.2f %.2f %.2f %.2f %.2f %.2f", positions[0], positions[1], positions[2], positions[3], positions[4], positions[5], positions[6]);

                  rb.Motors[0]->fresh_cmd(joint_direction[0] * positions[0], 0.0, 0, 500.0, 1); // q1
                  rb.Motors[1]->fresh_cmd(joint_direction[1] * positions[1], 0.0, 0, 500.0, 1); // q1
                  rb.Motors[2]->fresh_cmd(joint_direction[2] * positions[2], 0.0, 0, 500.0, 1); // q1
                  rb.Motors[3]->fresh_cmd(joint_direction[3] * positions[3], 0.0, 0, 200.0, 0.2); // q1
                  rb.Motors[4]->fresh_cmd(joint_direction[4] * positions[4], 0.0, 0, 200.0, 0.2); // q1
                  rb.Motors[5]->fresh_cmd(joint_direction[5] * positions[5], 0.0, 0, 200.0, 0.2); // q1
                  rb.Motors[6]->fresh_cmd(joint_direction[6] * positions[6], 0.0, 0, 200.0, 0.2); // q1
                  rb.motor_send();

                  // 步进
                  if (++current_step >= trajectory_segments[current_segment].totalSteps())
                  {
                        current_step = 0;
                        if (++current_segment >= trajectory_segments.size())
                        {
                              has_trajectory = false;
                              ROS_INFO("Trajectory complete");
                        }
                  }
            }

            // rb.Motors[0]->fresh_cmd(0, 0, 0, 0, 0);
            // rb.Motors[1]->fresh_cmd(0, 0, 0, 0, 0);
            // rb.Motors[2]->fresh_cmd(0, 0, 0, 0, 0);
            // rb.Motors[3]->fresh_cmd(0, 0, 0, 0, 0);
            // rb.Motors[4]->fresh_cmd(0, 0, 0, 0, 0);
            // rb.Motors[5]->fresh_cmd(0, 0, 0, 0, 0);
            // rb.Motors[6]->fresh_cmd(0, 0, 0, 0, 0);
            // rb.motor_send();

            // 发布话题
            for (int motor_index = 0; motor_index < 7; motor_index++)
            {
                  // 加入方向
                  joint_pos[motor_index] = joint_direction[motor_index] * rb.Motors[motor_index]->data.position;
            }

            joint_state.header.stamp = ros::Time::now();                                               // C++
            joint_state.name = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7"}; // 根据实际机器人关节修改
            joint_state.position = {joint_pos[0], joint_pos[1], joint_pos[2], joint_pos[3], joint_pos[4], joint_pos[5], joint_pos[6]};
            pub.publish(joint_state);

            // 周期循环
            ros::spinOnce();
            r.sleep();
            /* code */
      }

      // 释放
      for (auto &thread : rb.ser_recv_threads)
      {
            thread.join();
      }
}
